#include "stm32f10x.h"                  // Device header
#include "PWM.h"

/*
 * 高电平宽度(周期20ms)         舵机输出轴转角          对应占空比
 *    0.5ms                     0°                      0.5/20 = CCR / 20000 -> CCR = 500
 *    1ms                       45°                     1/20   = CCR / 20000 -> CCR = 1000
 *    1.5ms                     90°                     1.5/20 = CCR / 20000 -> CCR = 1500
 *    2ms                       135°                    2/20   = CCR / 20000 -> CCR = 2000
 *    2.5ms                     180°                    2.5/20 = CCR / 20000 -> CCR = 2500
 * 
 * pwm 占空比 = CCR / (ARR + 1)
*/

void servo_init(void)
{
    pwm_init();
}

/*
 * 角度计算：
 * 假设角度为x， 当x = 0, CCR = 500; x = 180, CCR = 2500;
 * CCR = kx + b , 带入计算可得角度值
*/
void servo_left_up(float angle)//左前
{
    pwm_set_compare_left_up(angle / 180 * 2000 + 500);
}

void servo_light_up(float angle)//右前
{
    pwm_set_compare_light_up((180 - angle) / 180 * 2000 + 500);
}

void servo_left_dowm(float angle)//左后
{
    pwm_set_compare_left_dowm(angle / 180 * 2000 + 500);
}

void servo_light_dowm(float angle) //右后
{
    pwm_set_compare_light_dowm((180 - angle) / 180 * 2000 + 500);
}

void servo_w(float angle)//尾巴
{
    pwm_set_compare_w(angle / 180 * 2000 + 500);
}
